# -*- coding: utf-8 -*-
import json
import socket
import os
import time

import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from sensor_msgs.msg import JointState

#2023.2-22正方向向上，负方向向下
class Brt0805aSrv(Node):
    def __init__(self):
        super().__init__('Brt0805aSrv')
        self.declare_parameter('arm_host', '192.168.1.4')
        self.declare_parameter('arm_port', 9760)
        
        self.arm_host_str = self.get_parameter('arm_host')
        self.arm_port_str = self.get_parameter('arm_port')
        
        j1,j2,j3,j4,j5,j6=0,0,0,0,0,0
        self.startbutton ='{"dsID":"HCRemoteMonitor","cmdType":"command","cmdData":["startButton"]}'
        self.stopbutton ='{"dsID":"HCRemoteMonitor","cmdType":"command","cmdData":["stopButton"]}'
        self.cmdjn ='{"dsID":"HCRemoteMonitor","cmdType":"command","cmdData":["rewriteDataList","800","6","0","0","0","-40000","0","0","0"]}'
        self.openM16 ='{"dsID":"HCRemoteMonitor","cmdType":"command","cmdData":["modifyOutput","4","6","1"]}'
        # 创建Socket对象
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # 连接服务器
        self.server_address = ('192.168.1.4', 9760)
        #self.server_address = self.arm_host_str + ":" + self.arm_port_str
        self.connected = False
        
        self.subscription = self.create_subscription(
            JointState,
            '/borunte_arm/joint_states',
            self.listener_callback,
            10)
        
    def listener_callback(self, msg):
        self.get_logger().info('j1=%f' % (msg.position[0] ) )
        self.get_logger().info('j2=%f' % (msg.position[1] ))
        self.get_logger().info('j3=%f' % (msg.position[2] ))
        self.get_logger().info('j4=%f' % (msg.position[3] ))
        self.get_logger().info('j5=%f' % (msg.position[4] ))
        self.get_logger().info('j6=%f' % (msg.position[5] ))
        #print(msg.position[0])
        
        if self.connected==False :
            self.connectArm()
            
        self.jnwrite(msg.position[0],msg.position[1],msg.position[2],msg.position[3],msg.position[4],msg.position[5])
        
    def connectArm(self):
        try: 
            print('Try to connnect to arm by Ethernet' )           
            self.client_socket.connect(self.server_address)   # Connect to server and send data
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            self.connected = True
            print('收到回复:', self.reply.decode())
        finally:
            print('call connectArm() end');
            #self.client_socket.close()
            
    def jnwrite(self,j1,j2,j3,j4,j5,j6):
        try:            
            #self.client_socket.connect(self.server_address)   # Connect to server and send data 
            #self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            #print('收到回复:', self.reply.decode())
            self.client_socket.sendall(self.startbutton.encode())    # 启动 
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            print('收到回复:', self.reply.decode())
            self.cmdjn ='{"dsID":"HCRemoteMonitor","cmdType":"command","cmdData":["rewriteDataList","800","6","0","0","0","-40000","0","0","0"]}'
            self.client_socket.sendall(self.cmdjn.encode())    # 填入坐标
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            print('收到回复:', self.reply.decode())
            self.client_socket.sendall(self.openM16.encode())    # 启动 
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            print('收到回复:', self.reply.decode())
        finally:
            #self.client_socket.close()
            print('call jnwrite(...) end ')
    def hex_to_signed_decimal(self,hex_str):
        decimal = int(hex_str, 16)
        if decimal > 0x7fffffff:
            decimal -= 0x100000000
        return decimal  
    def stop(self,hex_str):
        try:            
            self.client_socket.connect(self.server_address)   # Connect to server and send data 
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            print('收到回复:', self.reply.decode())
            self.client_socket.sendall(self.stopbutton.encode())    # 启动 
            self.reply = self.client_socket.recv(1024)    # 接收服务器回复消息
            print('收到回复:', self.reply.decode())
        finally:
            self.client_socket.close()
    def xyzuvwread(self):
        #pn000 参数编辑与初始化，默认值为1
        recived=self.ser.readall().hex()
        cmd=[0x01,0x03,0x09,0x1C,0x00,0x0c,0x87,0x95]
        #cmd=crc.AddCRC16(cmd)
        sendd=self.ser.write(cmd) #发数据
        recived=self.ser.readall().hex()
        recived1=[]
        for i in range(6,len(recived),8):
            a=self.hex_to_signed_decimal(recived[i:i+8])/1000
            recived1.append(a)
        return recived1

def main(args=None):
    rclpy.init(args=args)

    brt0805a_srv = Brt0805aSrv()

    rclpy.spin(brt0805a_srv)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    brt0805a_srv.destroy_node()
    rclpy.shutdown()

    
'''
#===============================================================
#   测试专用
#===============================================================
'''

if __name__ == '__main__':
    main()
    '''
    motor2=brt0805a()
    cmd=[0x01,0x05,0x09,0x1C,0x00,0x0c]
    #motor2.motorread([0x02, 0x04, 0x00, 0,0, 1])
    motor2.xyzuvwwrite(cmd)
    a=motor2.xyzuvwread()
    motor2.xyzuvwwrite(cmd)
    '''

'''
import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)





if __name__ == '__main__':
    main()
    
''' 
    
